#include "stdafx.h"
#include "IntrinsicSolver.h"
#include "least_square.h"
#include "nvmath.h"
#include <time.h>
#include <iostream>
#include "cvxSimilarColorQuadTree.h"

static const int IS_KNOWN = -1;  //index map

//principle channel
static const int RED_CHANNEL = 0;   // r =   9 - g - b
static const int GREEN_CHANNEL = 1; // num_g = sum%10
static const int BLUE_CHANNEL = 10; // num_b = sum/10

//local window size
static const int IS_WSIZE = 3;    //size of the local windows
static const int IS_WSIZE2 = 9;   //WSIZE2 = WSIZE*WSIZE, number of pixels in each window ('n' in the paper)
static const int IS_MSIZE = 12;   //WSIZE2+3, number of rows in the M matrix (see equation 8)
static const int IS_WSIZE_2 = 1;  //WSIZE_2 = WSIZE/2


using std::ofstream;
using std::ostream_iterator;
using cvx::Rgb8UImage;
using cvx::Bw8UImage;
using cvx::Bw32SImage;
using cvx::Bw32FImage;
using cvx::Rgb32FImage;
using nv::vec3f;


IntrinsicSolver::IntrinsicSolver()
{
	m_pEtree = NULL;
	m_pMtree = NULL;
}
IntrinsicSolver::~IntrinsicSolver()
{

}

void IntrinsicSolver::setImg(const Mat &img)
{
	assert(!img.empty());
	assert(img.channels() == 3);
	int w = img.cols;
	int h = img.rows;
	m_orgImg  = img.clone();
	m_edgeMap = cv::Mat::zeros(h, w, CV_8U);
	m_miniShading = cv::Mat::zeros(h, w, CV_32F);
	computeMiniShading();
}

bool IntrinsicSolver::isEmpty(void)
{
	return m_orgImg.empty();
}
int IntrinsicSolver::width(void)
{
	return m_orgImg.cols;
}
int IntrinsicSolver::height(void)
{
	return m_orgImg.rows;
}
void IntrinsicSolver::GetOrgImg(Mat &img)
{
	img = m_orgImg.clone();
}

void IntrinsicSolver::GetEdgemap(Mat &edgemap, const cv::Rect *pRect)
{
	if (pRect)
	{
		edgemap = m_edgeMap(*pRect).clone();
	}
	else
	{
		edgemap = m_edgeMap.clone();
	}
}
void IntrinsicSolver::GetMiniShading(Mat &miniSha, const cv::Rect *pRect)
{
	if (pRect)
	{
		miniSha = m_miniShading(*pRect).clone();
	}
	else
	{
		miniSha = m_miniShading;
	}
}


bool IntrinsicSolver::initSolveLocalShading(const Mat &strokeMap, int label, const Mat &edgeMap, 
											const Mat &img)
{
	assert(!strokeMap.empty());
	assert(label > 0);
	assert(!img.empty());
	assert(!edgeMap.empty());
	assert(strokeMap.rows == edgeMap.rows && strokeMap.rows == img.rows);

	int w = img.cols;
	int h = img.rows;

	Mat idxMap = cv::Mat::zeros(h,w,CV_32S);
	Mat shadingMap = cv::Mat::zeros(h, w, CV_32F);
	Mat reflectanceMap = cv::Mat::zeros(h, w, CV_8UC3);

	int unknowNum = 0;
	int avgCol[3] = {0, 0, 0};
	float s_min = 0.0;       //possible minimum shading value in local patch

	//get index map of unknowns
	GetIndexMap(strokeMap, label, img, idxMap, unknowNum, s_min, avgCol);

	//get fixed shading value for knowns
	GetFixedShading(idxMap, img, s_min, avgCol, shadingMap, reflectanceMap);

	cout<<"s_min = "<<s_min<<endl;

	double tt = clock();
	vector<map<int, double>> matrix_vec;
	vector<double> right_vec;
	//set equation where there is not edge
	GetEquationAtLocalWindow(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec, true);
	GetEquationAtEdge(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec);
	cout<<"collect data cost time "<<clock()-tt<<endl;

	tt = clock();
	vector<double> result(unknowNum);
	bool isOk = m_lSolver._solver.initialSolverLeft(matrix_vec, unknowNum);
	if (!isOk)
	{
		cout<<"initialSolverLeft failed\n";
		return false;
	}
	isOk = m_lSolver._solver.solverRight(right_vec, result);
	if (!isOk)
	{
		cout<<"solverRight failed\n";
		return false;
	}
	cout<<"solver cost time "<<clock()-tt<<endl;
	if (isOk)
	{
		Rgb8UImage org_img(&img);
		Bw32SImage idx_map(&idxMap);
		Bw32FImage sha_map(&shadingMap);
		Rgb8UImage ref_map(&reflectanceMap);

		for (int y = 0; y<h; ++y)
		{
			for (int x = 0; x<w; ++x)
			{
				if (idx_map[y][x] != IS_KNOWN)
				{
					sha_map[y][x] = result[idx_map[y][x]];					
				}				
			}
		}
		for (int y = 0; y<h; ++y)
		{
			for (int x =0 ; x<w; ++x)
			{
				float sVal = sha_map[y][x];
				if (sVal > 0.0)
				{
					ref_map[y][x].r = org_img[y][x].r /sVal;
					ref_map[y][x].g = org_img[y][x].g /sVal;
					ref_map[y][x].b = org_img[y][x].b /sVal;
				}
			}
		}

		//store data
		m_lSolver._shadingMap = shadingMap;
		m_lSolver._reflectanceMap = reflectanceMap;
		m_lSolver._idxMap = idxMap;
		m_lSolver._edgeMap = edgeMap;
		m_lSolver._orgImg  = img;
		m_lSolver._avgCol[0] = avgCol[0];
		m_lSolver._avgCol[1] = avgCol[1];
		m_lSolver._avgCol[2] = avgCol[2];
		m_lSolver._unknowNum = unknowNum;
		m_lSolver._baseLine = s_min;
		m_lSolver._step = (1.0-s_min)/10.0;
		

   //		Mat showShading = shadingMap.clone();
   	//	showShading.convertTo(showShading, CV_8U, 255.0);
   	//	imshow("reflectance image", reflectanceMap);
   	//	imshow("shading image", showShading);
   //		cvWaitKey(0);
		return true;
	}
	else
	{
		return false;
	}		
}


bool IntrinsicSolver::updateSolveLocalShading(float minShading, bool showResult)
{

	//get data from previous iterator
	Mat shadingMap = m_lSolver._shadingMap ;
	Mat reflectanceMap = m_lSolver._reflectanceMap;
	Mat idxMap = m_lSolver._idxMap;
	Mat edgeMap = m_lSolver._edgeMap;
	Mat img = m_lSolver._orgImg;
	int avgCol[3];
	avgCol[0] = m_lSolver._avgCol[0];
	avgCol[1] = m_lSolver._avgCol[1];
	avgCol[2] = m_lSolver._avgCol[2];
	int unknowNum = m_lSolver._unknowNum;

	assert(!img.empty());
	assert(!edgeMap.empty());
	
	int w = img.cols;
	int h = img.rows;

	//get fixed shading value for knowns
	GetFixedShading(idxMap, img, minShading, avgCol, shadingMap, reflectanceMap);

	double tt = clock();
	vector<map<int, double>> matrix_vec;
	vector<double> right_vec;
	//set equation where there is not edge
	GetEquationAtLocalWindow(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec, false);
	GetEquationAtEdge(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec);
	cout<<"collect data cost time "<<clock()-tt<<endl;

	tt = clock();
	vector<double> result(unknowNum);
	bool isOk = false;

	isOk = m_lSolver._solver.solverRight(right_vec, result);
	if (!isOk)
	{
		cout<<"solverRight failed"<<endl;
		return false;
	}
	cout<<"solver cost time "<<clock()-tt<<endl;
	if (isOk)
	{
		Rgb8UImage org_img(&img);
		Bw32SImage idx_map(&idxMap);
		Bw32FImage sha_map(&shadingMap);
		Rgb8UImage ref_map(&reflectanceMap);

		for (int y = 0; y<h; ++y)
		{
			for (int x = 0; x<w; ++x)
			{
				if (idx_map[y][x] != IS_KNOWN)
				{
					sha_map[y][x] = result[idx_map[y][x]];					
				}				
			}
		}
		for (int y = 0; y<h; ++y)
		{
			for (int x =0 ; x<w; ++x)
			{
				float sVal = sha_map[y][x];
				if (sVal > 0.0)
				{
					ref_map[y][x].r = org_img[y][x].r /sVal;
					ref_map[y][x].g = org_img[y][x].g /sVal;
					ref_map[y][x].b = org_img[y][x].b /sVal;
				}
			}
		}
		//show result
		if(showResult)
		{
			Mat showShading = shadingMap.clone();
 			showShading.convertTo(showShading, CV_8U, 255.0);
 			imshow("reflectance image", reflectanceMap);
 			imshow("shading image", showShading);
 			cvWaitKey(0);		 		
		}

	}
	else
	{
		return false;
	}		

	return true;
}
void IntrinsicSolver::GetLocalShadingBaselineAndStep(float &baseline, float &step)
{
	baseline = m_lSolver._baseLine;
	step = m_lSolver._step;
}

void IntrinsicSolver::GetLocalShadingMap(Mat &outMat)
{
	assert(outMat.rows == m_lSolver._shadingMap.rows);
	m_lSolver._shadingMap.copyTo(outMat);
}
void IntrinsicSolver::GetLocalReflectanceMap(Mat &outMat)
{
	assert(outMat.rows == m_lSolver._reflectanceMap.rows);
	m_lSolver._reflectanceMap.copyTo(outMat);
}

/*
bool IntrinsicSolver::ExpandShading(const Mat &img, const Mat &knownMap, const Mat &edgeMap, 
									Mat &shadingMap, Mat &reflectanceMap, int &errorCode)
{
	assert(shadingMap.depth() == CV_32F);

	int w = img.cols;
	int h = img.rows;

	Mat idxMap = cv::Mat::zeros(h, w, CV_32S);

	//index map
	Bw32SImage idx_map(&idxMap);
	Bw8UImage kn_map(&knownMap);
	int unknownNum = 0;
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			if (kn_map[y][x] == 255)
			{
				idx_map[y][x] = -1;				
			}
			else
			{
				idx_map[y][x] = unknownNum++;				
			}
		}
	}
	if (w*h - unknownNum < 50)
	{
		errorCode = 1;
		return false;
	}
	vector<map<int, double>> matrix_vec;
	vector<double> right_vec;

	//set equation in non-edge area
	GetEquationAtLocalWindow(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec, true); 
	GetEquationAtEdge(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec);
	assert(matrix_vec.size() == right_vec.size());

	vector<double> result(unknownNum);
	LeastSquare solver;

	bool isOk = false;	
	isOk = solver.initialSolverLeft(matrix_vec, unknownNum);
	if (isOk)
	{
		isOk = solver.solverRight(right_vec, result);
	}	
	if (isOk)
	{

		Rgb8UImage org_img(&img);
		Bw32SImage idx_map(&idxMap);
		Bw32FImage sha_map(&shadingMap);
		Rgb8UImage ref_map(&reflectanceMap);

		for (int y = 0; y<h; ++y)
		{
			for (int x = 0; x<w; ++x)
			{
				if (idx_map[y][x] != IS_KNOWN)
				{
					sha_map[y][x] = result[idx_map[y][x]];					
				}				
			}
		}
		for (int y = 0; y<h; ++y)
		{
			for (int x =0 ; x<w; ++x)
			{
				float sVal = sha_map[y][x];
				if (sVal > 0.0)
				{
					ref_map[y][x].r = org_img[y][x].r /sVal;
					ref_map[y][x].g = org_img[y][x].g /sVal;
					ref_map[y][x].b = org_img[y][x].b /sVal;
				}
			}
		}

		//show result
  		 Mat showShading = shadingMap.clone();
  		 showShading.convertTo(showShading, CV_8U, 255.0);
  		 imshow("reflectance image", reflectanceMap);
  		 imshow("shading image", showShading);
  		 cvWaitKey(0);

		 errorCode = 0;
		 return true;
	}
	else
	{
		errorCode = 2;
		return false;
	}
}

bool IntrinsicSolver::IterativeExpanding(const Mat &img, const Mat &knownMap, const Mat &edgeMap, 
						const Mat &minimalShading, Mat &shadingMap, Mat &reflectanceMap, int &errorCode)
{
	assert(shadingMap.depth() == CV_32F);
	assert(minimalShading.depth() == CV_32F);

	int w = img.cols;
	int h = img.rows;

	Mat idxMap = cv::Mat::zeros(h, w, CV_32S);

	//index map
	Bw32SImage idx_map(&idxMap);
	Bw8UImage kn_map(&knownMap);
	int unknownNum = 0;
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			if (kn_map[y][x] == 255)
			{
				idx_map[y][x] = -1;				
			}
			else
			{
				idx_map[y][x] = unknownNum++;				
			}
		}
	}
	if (w*h - unknownNum < 50)
	{
		errorCode = 1;
		return false;
	}
	vector<map<int, double>> matrix_vec;
	vector<double> right_vec;

	//set equation in non-edge area
	GetEquationAtLocalWindow(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec, true); 
	GetEquationAtEdge(idxMap, shadingMap, edgeMap, img, matrix_vec, right_vec);

	//add more weight to edge	
	assert(matrix_vec.size() == right_vec.size());

	vector<double> result(unknownNum);
	LeastSquare solver;

	bool isOk = false;	
	isOk = solver.initialSolverLeft(matrix_vec, unknownNum);
	if (isOk)
	{
		isOk = solver.solverRight(right_vec, result);
	}	
	if (isOk)
	{

		Rgb8UImage org_img(&img);
		Bw32SImage idx_map(&idxMap);
		Bw32FImage sha_map(&shadingMap);
		Rgb8UImage ref_map(&reflectanceMap);

		for (int y = 0; y<h; ++y)
		{
			for (int x = 0; x<w; ++x)
			{
				if (idx_map[y][x] != IS_KNOWN)
				{
					sha_map[y][x] = result[idx_map[y][x]];					
				}				
			}
		}
		for (int y = 0; y<h; ++y)
		{
			for (int x =0 ; x<w; ++x)
			{
				float sVal = sha_map[y][x];
				if (sVal > 0.0)
				{
					ref_map[y][x].r = org_img[y][x].r /sVal;
					ref_map[y][x].g = org_img[y][x].g /sVal;
					ref_map[y][x].b = org_img[y][x].b /sVal;
				}
			}
		}

		Mat errorMap = cv::Mat::zeros(h, w, CV_8U);
		Mat rectifyMap = cv::Mat::zeros(h, w, CV_32F);
		Bw8UImage  e_map(&errorMap);
		Bw32FImage min_sha(&minimalShading);
		Bw32FImage rec_map(&rectifyMap);
		int errorNum = 0;
		for (int y = 0; y<h; ++y)
		{
			for (int x =0; x<w; ++x)
			{
				if (sha_map[y][x] < min_sha[y][x])
				{
					e_map[y][x] = 255;
					rec_map[y][x] = min_sha[y][x] + (min_sha[y][x] - sha_map[y][x]);
					errorNum++;
				}
			}
		}

// 		Mat showShading = shadingMap.clone();
// 		showShading.convertTo(showShading, CV_8U, 255.0);
// 		imshow("reflectance image 1", reflectanceMap);
// 		imshow("shading image 1", showShading);
// 		imshow("error map 1", errorMap);
// 		cvWaitKey(0);

		//
		if (errorNum > 0)
		{
			CorrectErrorMap(idxMap, errorMap, rectifyMap, matrix_vec, right_vec);
			isOk = solver.initialSolverLeft(matrix_vec, unknownNum);
			if (isOk)
			{
				isOk = solver.solverRight(right_vec, result);
			}
			else
			{
				errorCode = 3;
				return false;
			}
			if (isOk)
			{
				for (int y = 0; y<h; ++y)
				{
					for (int x = 0; x<w; ++x)
					{
						if (idx_map[y][x] != IS_KNOWN)
						{
							sha_map[y][x] = result[idx_map[y][x]];					
						}				
					}
				}
				for (int y = 0; y<h; ++y)
				{
					for (int x =0 ; x<w; ++x)
					{
						float sVal = sha_map[y][x];
						if (sVal > 0.0)
						{
							ref_map[y][x].r = org_img[y][x].r /sVal;
							ref_map[y][x].g = org_img[y][x].g /sVal;
							ref_map[y][x].b = org_img[y][x].b /sVal;
						}
					}
				}

				//show result
// 				Mat showShading = shadingMap.clone();
// 				showShading.convertTo(showShading, CV_8U, 255.0);
 				imshow("reflectance image", reflectanceMap);
// 				imshow("shading image", showShading);
// 				imshow("error map", errorMap);
 				cvWaitKey(0);

				errorCode = 0;
				return true;
			}
			else
			{
				errorCode = 4;
				return false;
			}
		}
	}
	else
	{
		errorCode = 2;
		return false;
	}
}

*/

bool IntrinsicSolver::setETree(const Mat &img, const Mat &edgeMap, const Mat &knownMap, 
							  Mat &shadingMap, Mat &reflectanceMap)
{
	assert(shadingMap.depth() == CV_32F);
	int w = img.cols;
	int h = img.rows;
	assert(w == h);

	//@todo
	int level = 0;
	int unknownNum = 0;
	while((1 << level) < w)
	{
		level++;		
	}
	level++;
	m_pEtree = new CvxSimColorQuadTree(level, img, 5);
	m_pEtree->create(NULL);
	m_pEtree->balance();
	m_pEtree->setLeafData(img, shadingMap, knownMap, NULL, edgeMap);

// 	m_pEtree->drawMiniShading(shadingMap, NULL);
// 	Mat showShadingmap = Mat(h, w, CV_8U);
// 	shadingMap.convertTo(showShadingmap, CV_8U, 255.0);
// 	imshow("minimal shading value", showShadingmap);

//  	m_pEtree->draw(NULL);
//  	imshow("balanced tree", m_pEtree->m_img);
//  	cvWaitKey(0);

	return true;
}

bool IntrinsicSolver::ETreeSolver(const Mat &img, Mat &shadingMap, Mat &reflectanceMap)
{
	assert(shadingMap.depth() == CV_32F);
	int w = img.cols;
	int h = img.rows;
	assert(w == h);
	assert(m_pEtree);

 	int unknownNum = m_pEtree->m_unknownNum;

	// calculate shading value with quadtree
	vector<map<int, double>> leftMap;
	vector<double> rightVec;

	double tt = clock();
	m_pEtree->setPlaneEquation(NULL, leftMap, rightVec);
	cout<<"set plane equation cost time "<<clock() - tt<<endl;
	tt = clock();
	m_pEtree->setEdgeEquation(NULL, leftMap, rightVec);
	cout<<"set edge equation cost time "<<clock() - tt<<endl;

	assert(leftMap.size() == rightVec.size());

	unknownNum = m_pEtree->m_unknownNum;
	vector<double> result(unknownNum);
	if (unknownNum != 0)
	{	
		double tt = clock();
//		bool isOk = CvxLeastSquare::solver(leftMap, rightVec, true, unknownNum, &result[0]);		
		bool isOk = m_lSolver._solver.initialSolverLeft(leftMap, unknownNum);
		if (!isOk)
		{
			cout<<"initialSolverLeft failed\n";
			return false;
		}
		isOk = m_lSolver._solver.solverRight(rightVec, result);
		if (!isOk)
		{
			cout<<"solverRight failed\n";
			return false;
		}
		cout<<"E solver cost time = "<<clock()-tt<<endl;
	}
	else
	{
		cout<<"kk unknown number is zero"<<endl;
	}	
	m_pEtree->setShadingResult(shadingMap, NULL, result);
	return true;	
}

bool IntrinsicSolver::setMTree(bool reverse)
{
	assert(m_pEtree);
	if (m_pMtree == NULL)
	{
		m_pMtree = new CvxSimColorQuadTree(*m_pEtree);
		if (reverse)
		{
			m_pMtree->reverse(NULL);
		}	
	}
	return true;
}

bool IntrinsicSolver::MTreeSolver(const Mat &img, Mat &shadingMap, Mat &reflectanceMap)
{
	
	// calculate shading value with quadtree
	

	int unknownNum = m_pMtree->m_unknownNum;
	vector<double> result(unknownNum);
	if (unknownNum != 0)
	{
		vector<map<int, double>> leftMap;
		vector<double> rightVec;
		m_pMtree->setPlaneEquation(NULL, leftMap, rightVec);
		m_pMtree->setEdgeEquation(NULL, leftMap, rightVec);

		assert(leftMap.size() == rightVec.size());

		bool isOk = m_lSolver._solver.initialSolverLeft(leftMap, unknownNum);
		if (!isOk)
		{
			cout<<"initialSolverLeft failed\n";
			return false;
		}
		isOk = m_lSolver._solver.solverRight(rightVec, result);
		if (!isOk)
		{
			cout<<"solverRight failed\n";
			return false;
		}
	}
	else
	{
		printf("unknown number is %d\n", unknownNum);
	}

	m_pMtree->setShadingResult(shadingMap, NULL, result);

	return true;
}

bool IntrinsicSolver::UpSampleAVector(const Mat &img, Mat &shadingMap, Mat &reflectanceMap)
{
	assert(m_pEtree);
	assert(m_pMtree);

	int w = shadingMap.cols;
	int h = shadingMap.rows;

	int unknownNum = m_pMtree->m_nodeNum;

	//calculate A vector
	vector<map<int, double>> leftMap;
	vector<double> rightVec;

	
	m_pMtree->setAvectorEquation(NULL, leftMap, rightVec);	

	vector<double> result(unknownNum*3);
	bool isOk = m_lSolver._solver.initialSolverLeft(leftMap, unknownNum*3);
	if (!isOk)
	{
		cout<<"initialSolverLeft failed\n";
		return false;
	}
	isOk = m_lSolver._solver.solverRight(rightVec, result);
	if (!isOk)
	{
		cout<<"solverRight failed\n";
		return false;
	}

	Mat AvectorMatrix = Mat(h, w, CV_32FC3, Scalar(0, 0, -255, 0));
	m_pMtree->drawAvector(AvectorMatrix, NULL, result);


	Bw32FImage sha_map(&shadingMap);
	Rgb32FImage avector_map(&AvectorMatrix);
	Rgb8UImage org_img(&img);
	for (int y = 0; y<h; ++y)
	{
		for (int x =0; x<w; ++x)
		{
		//	if (avector_map[y][x].r != -255)
			{
				sha_map[y][x] = org_img[y][x].r * avector_map[y][x].r + 
					org_img[y][x].g * avector_map[y][x].g + 
					org_img[y][x].b * avector_map[y][x].b;
				sha_map[y][x] = std::min(1.0f, sha_map[y][x]);
			}			
		}
	}
	return true;

}

bool IntrinsicSolver::GetColorPlaneNormalFromShading(const Mat &img, const Mat &shadingMap, Mat &AMap)
{
	assert(shadingMap.depth() == CV_32F);
	assert(AMap.depth() == CV_32F && AMap.channels() == 3);

	int w = img.cols;
	int h = img.rows;
	double epsilon = 0.000001;
	double sqrt_epsilon = sqrt(epsilon);

	vector<map<int, double>> leftVec;
	vector<double> rightVec;

	Rgb8UImage org_img(&img);
	Bw32FImage s_map(&shadingMap);
	for (int y=1; y<h-1; ++y)
	{
		for (int x=1; x<w-1; ++x)
		{
			int sIdx = (y-1)*(w-2) + (x-1);
			for (int m=-1; m<=1; ++m)
			{
				for (int n=-1; n<=1; ++n)
				{
					int sx = x+n;
					int sy = y+m;
					
					int r, g, b;
					r = org_img[sy][sx].r;
					g = org_img[sy][sx].g;
					b = org_img[sy][sx].b;

					map<int, double> imap;
					double rightVal = s_map[sy][sx];
					imap[sIdx*3+0] = r;
					imap[sIdx*3+1] = g;
					imap[sIdx*3+2] = b;
					leftVec.push_back(imap);
					rightVec.push_back(rightVal);
				}
			}
			{
				map<int, double> imap;
				imap[sIdx*3+0] = sqrt_epsilon;
				leftVec.push_back(imap);
				rightVec.push_back(0.0);
			}
			{
				map<int, double> imap;
				imap[sIdx*3+1] = sqrt_epsilon;
				leftVec.push_back(imap);
				rightVec.push_back(0.0);
			}
			{
				map<int, double> imap;
				imap[sIdx*3+2] = sqrt_epsilon;
				leftVec.push_back(imap);
				rightVec.push_back(0.0);
			}

		}
	}

	assert(leftVec.size() == rightVec.size());
	cout<<"leftVec size = "<<leftVec.size()<<endl;

	int unknownNum = (w-2) * (h-2) * 3;
	vector<double> result(unknownNum);
	bool isOk = m_lSolver._solver.initialSolverLeft(leftVec, unknownNum);
	if (!isOk)
	{
		cout<<"initialSolverLeft failed\n";
		return false;
	}
	isOk = m_lSolver._solver.solverRight(rightVec, result);
	if (!isOk)
	{
		cout<<"solverRight failed\n";
		return false;
	}	

	Rgb32FImage a_map(&AMap);
	for (int y=1; y<h-1; ++y)
	{
		for (int x=1; x<w-1; ++x)
		{
			int sIdx = (y-1)*(w-2) + (x-1);
			a_map[y][x].r = result[3*sIdx+0];
			a_map[y][x].g = result[3*sIdx+1];			
			a_map[y][x].b = result[3*sIdx+2];
		}
	}

	//get shading
	Mat sMap = cv::Mat::zeros(h, w, CV_8U);
	Bw8UImage s2_map(&sMap);
	for (int y = 1; y<h-1; ++y)
	{
		for (int x = 1; x<w-1; ++x)
		{
			float a1,a2,a3;
			a1 = a_map[y][x].r;
			a2 = a_map[y][x].g;
			a3 = a_map[y][x].b;
			s2_map[y][x] = (a1 * org_img[y][x].r + a2 * org_img[y][x].g + a3 * org_img[y][x].b) * 255;
		}
	}
	imshow("shading from a dot I", sMap);	
	return true;
}

void IntrinsicSolver::computeEdgeMap(float threshold)
{
	assert(!m_orgImg.empty());
	assert(!m_edgeMap.empty());

	int w = m_orgImg.cols;
	int h = m_orgImg.rows;

	Rgb8UImage org_img(&m_orgImg);
	Bw8UImage edge_map(&m_edgeMap);
	for (int y = 1; y<h-1; ++y)
	{
		for (int x = 1; x<w-1; ++x)
		{
			float v[3];
			float top[3], bot[3], left[3], right[3];
			v[0] = org_img[y][x].r;
			v[1] = org_img[y][x].g;
			v[2] = org_img[y][x].b;

			top[0] = org_img[y-1][x].r;
			top[1] = org_img[y-1][x].g;
			top[2] = org_img[y-1][x].b;

			bot[0] = org_img[y+1][x].r;
			bot[1] = org_img[y+1][x].g;
			bot[2] = org_img[y+1][x].b;

			left[0] = org_img[y][x-1].r;
			left[1] = org_img[y][x-1].g;
			left[2] = org_img[y][x-1].b;

			right[0] = org_img[y][x+1].r;
			right[1] = org_img[y][x+1].g;
			right[2] = org_img[y][x+1].b;

			vec3f vf(v);
			vec3f v1f(top);
			vec3f v2f(bot);
			vec3f v3f(left);
			vec3f v4f(right);

			vf = nv::normalize(vf);
			v1f = nv::normalize(v1f);
			v2f = nv::normalize(v2f);
			v3f = nv::normalize(v3f);
			v4f = nv::normalize(v4f);

			float angle = nv::dot(vf, v1f);
			if (angle < threshold)
			{
				edge_map[y][x] = 255;				
			}
			else 
			{
				angle = nv::dot(vf, v2f);
				if (angle < threshold)
				{
					edge_map[y][x] = 255;
				}
				else
				{
					angle = nv::dot(vf, v3f);
					if (angle < threshold)
					{
						edge_map[y][x] = 255;
					}
					else
					{
						angle = nv::dot(vf, v4f);
						if (angle < threshold)
						{
							edge_map[y][x] = 255;
						}
					}
				}
			}
		}
	}
//	imshow("edge map", m_edgeMap);
//	cvWaitKey(0);
}

void IntrinsicSolver::computeMiniShading()
{
	assert(!m_orgImg.empty());
	assert(m_miniShading.depth() == CV_32F);

	int w = m_orgImg.cols;
	int h = m_orgImg.rows;

	Rgb8UImage org_img(&m_orgImg);
	Bw32FImage min_sha(&m_miniShading);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			int r,g,b;
			r = org_img[y][x].r;
			g = org_img[y][x].g;
			b = org_img[y][x].b;
			r = std::max(r, g);
			r = std::max(r, b);
			min_sha[y][x] = r/255.0;
		}
	}



}

void IntrinsicSolver::SegmentPath(const vector<cv::Point> &path, float threshold, 
								  vector<vector<cv::Point>> &pathes)
{
	//part the path into two segment greedily
	assert(!path.empty());
	assert(!m_orgImg.empty());

	if (path.size() < 10)
	{
		return;
	}

	vector<int> difArray;
	Rgb8UImage org_img(&m_orgImg);
	for (int i = 1; i<path.size(); i += 1)
	{
		int px = path[i-1].x;
		int py = path[i-1].y;
		int x = path[i].x;
		int y = path[i].y;

		int r_dif = org_img[py][px].r - org_img[y][x].r;
		int g_dif = org_img[py][px].g - org_img[y][x].g;
		int b_dif = org_img[py][px].b - org_img[y][x].b;
		int dif = r_dif * r_dif + g_dif * g_dif + b_dif * b_dif;
 		
		difArray.push_back(dif);
	}	
	vector<int>::iterator iteMax = std::max_element(difArray.begin(), difArray.end());
	if (iteMax - difArray.begin() <= 4)
	{
		return;
	}
	if (difArray.end() - iteMax <= 4)
	{
		return;
	}	
	int step = iteMax - difArray.begin();

	vector<cv::Point>::const_iterator iteDiv = path.begin() + step;
	vector<cv::Point> leftPath(iteDiv-2-path.begin());
	vector<cv::Point> rightPath(path.end() - iteDiv -2);
	std::copy(path.begin(), iteDiv - 2, leftPath.begin());
	std::copy(iteDiv+2, path.end(), rightPath.begin());	

	pathes.clear();
	pathes.push_back(leftPath);
	pathes.push_back(rightPath);

// 	Mat dotMat(dotArray);
// 	FileStorage fs("dotArray.xml", FileStorage::WRITE);	
// 	fs<<"dotArray"<<dotArray;
// 	fs.release();
}

cv::Rect IntrinsicSolver::GetBoundingBox(int w, int h, int penWidth, 
										 const vector<cv::Point> &pathPts, int expandWidth)
{
	assert(!pathPts.empty());
	assert(w > 0 && h > 0);
	assert(expandWidth > 0);
	assert(expandWidth%4 == 0);

	cv::Rect r;
	Point pmin(w, h);
	Point pmax(0, 0);

	for (int i = 0; i<pathPts.size(); ++i)
	{
		int x = pathPts[i].x, y = pathPts[i].y;
		Point p1, p2;
		p1.x = x - penWidth;
		p1.y = y - penWidth;
		p2.x = x + penWidth;
		p2.y = y + penWidth;

		pmin.x = std::min(pmin.x, p1.x);
		pmin.y = std::min(pmin.y, p1.y);
		pmax.x = std::max(pmax.x, p2.x);
		pmax.y = std::max(pmax.y, p2.y);
	}

	pmax.x += 4;
	pmax.y += 4;

	pmin.x -= expandWidth/2;
	pmin.y -= expandWidth/2;

	pmax.x += expandWidth/2;
	pmax.y += expandWidth/2;

	pmin.x = std::max(0, pmin.x);
	pmin.y = std::max(0, pmin.y);

	pmax.x = std::min(pmax.x, w);
	pmax.y = std::min(pmax.y, h);

	r.x = pmin.x;
	r.y = pmin.y;
	r.width  = pmax.x - pmin.x;
	r.height = pmax.y - pmin.y;

	r.width -= r.width%4;
	r.height -= r.height%4;	
	return r;
}

void IntrinsicSolver::GetIndexMap(const Mat &strokeMap, int label, const Mat &img, 
								   Mat &indexMap, int &unKnownNum, float &minShading, int *avgCol)
{
	assert(indexMap.depth() == CV_32S);

	int w = img.cols;
	int h = img.rows;
	avgCol[0] = 0;
	avgCol[1] = 0;
	avgCol[2] = 0;
	unKnownNum = 0;

	Bw8UImage stroke_map(&strokeMap);
	Rgb8UImage org_img(&img);
	Bw32SImage idx_map(&indexMap);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			// unknown position
			if (stroke_map[y][x] != label)
			{
				idx_map[y][x] = unKnownNum++;					
			}
			else
			{
				idx_map[y][x] = IS_KNOWN;
				avgCol[0] += org_img[y][x].r;
				avgCol[1] += org_img[y][x].g;
				avgCol[2] += org_img[y][x].b;
			}
		}
	}
	assert(w * h - unKnownNum > 10);
	avgCol[0] /= w * h - unKnownNum;
	avgCol[1] /= w * h - unKnownNum;
	avgCol[2] /= w * h - unKnownNum;

	int vc_max = std::max(avgCol[0], avgCol[1]);
	vc_max = std::max(vc_max, avgCol[2]);
	minShading = vc_max / 255.0;
}

void IntrinsicSolver::GetFixedShading(const Mat &idxMap, const Mat &orgImage,
									   float minShading, int *avgCol, Mat &shadingMap, Mat &reflectanceMap)
{
	assert(shadingMap.depth() == CV_32F);
	assert(idxMap.depth() == CV_32S);
	assert(minShading > 0 && minShading <= 1.0);

	int w = orgImage.cols;
	int h = orgImage.rows;

	float ref_c[3];
	ref_c[0] = avgCol[0]/minShading;
	ref_c[1] = avgCol[1]/minShading;
	ref_c[2] = avgCol[2]/minShading;

	Bw32FImage sha_map(&shadingMap);
	Rgb8UImage ref_map(&reflectanceMap);
	Rgb8UImage org_img(&orgImage);
	Bw32SImage idx_map(&idxMap);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0;x<w; ++x)
		{
			if (idx_map[y][x] == IS_KNOWN)
			{
				float s_r = org_img[y][x].r/ref_c[0];
				float s_g = org_img[y][x].g/ref_c[1];
				float s_b = org_img[y][x].b/ref_c[2];
				float s = std::max(s_r, s_g);
				s = std::max(s, s_b);
				ref_map[y][x].r = org_img[y][x].r/s;
				ref_map[y][x].g = org_img[y][x].g/s;
				ref_map[y][x].b = org_img[y][x].b/s;

				sha_map[y][x] = s;   //get part of shading map
			}
		}
	}
}

void IntrinsicSolver::GetEquationAtLocalWindow(const Mat &idxMap, const Mat &shadingMap, 
											   const Mat &edgeMap, const Mat &orgImage, 
											   vector<map<int, double>> &matrix_vec, 
											   vector<double> &right_vec, bool needLeft)
{
	assert(idxMap.depth() == CV_32S);
	assert(shadingMap.depth() == CV_32F);

	int w = orgImage.cols;
	int h = orgImage.rows;
	double epsilon = 0.01;
	double sqrt_epsilon = sqrt(epsilon);

	/* Set M */
	double M[IS_MSIZE][3]; //M matrix (see equation 8)
	M[IS_MSIZE-3][0] = sqrt_epsilon; M[IS_MSIZE-3][1] = 0; M[IS_MSIZE-3][2] = 0;
	M[IS_MSIZE-2][0] = 0; M[IS_MSIZE-2][1] = sqrt_epsilon; M[IS_MSIZE-2][2] = 0;
	M[IS_MSIZE-1][0] = 0; M[IS_MSIZE-1][1] = 0; M[IS_MSIZE-1][2] = sqrt_epsilon;

	Bw8UImage edge_map(&edgeMap);
	Rgb8UImage org_img(&orgImage);
	Bw32FImage sha_map(&shadingMap);
	Bw32SImage idx_map(&idxMap);
	for (int y = IS_WSIZE_2; y<h-IS_WSIZE_2; ++y)
	{
		for (int x = IS_WSIZE_2; x<w-IS_WSIZE_2; ++x)
		{
			//it is not an edge
			if (edge_map[y][x] == 0)
			{
				//fill the M matrix with the image values in the current window
				//Equ 8
				for (int m=-IS_WSIZE_2; m<=IS_WSIZE_2; m++) 
				{
					int yC = y+m;
					for (int n=-IS_WSIZE_2; n<=IS_WSIZE_2; n++)
					{
						int xC = x+n;
						M[(m+IS_WSIZE_2)*IS_WSIZE + n+IS_WSIZE_2][0] = org_img[yC][xC].r;
						M[(m+IS_WSIZE_2)*IS_WSIZE + n+IS_WSIZE_2][1] = org_img[yC][xC].g;
						M[(m+IS_WSIZE_2)*IS_WSIZE + n+IS_WSIZE_2][2] = org_img[yC][xC].b;
					}
				}
				//get Ni
				Mat Mi  = Mat(IS_MSIZE,3,CV_64F, M);
				Mat Mit = Mi.t();
				Mat MitMi = Mit * Mi;
				Mat MitMi_inv = MitMi.inv();
				Mat Ni = Mi * MitMi_inv * Mit;
				Ni = Mat::eye(Ni.rows, Ni.cols, CV_64F) - Ni;	

				double *pNi = (double*)(Ni.data);

				if (needLeft)
				{
					for (int j = 0; j<Ni.rows; ++j)
					{
						map<int, double> imap;
						double rightVal = 0.0;					
						for (int k = 0; k<Ni.cols-3; ++k)
						{
							int niIdx = j * Ni.cols + k;

							//sIdx position in original map
							int sy = k/IS_WSIZE - IS_WSIZE_2 + y;
							int sx = k%IS_WSIZE - IS_WSIZE_2 + x;
							//shading value already known
							if (idx_map[sy][sx] == IS_KNOWN)
							{
								rightVal -= pNi[niIdx] * sha_map[sy][sx];								
							}
							else
							{						
								int sIdx = idx_map[sy][sx];
								imap[sIdx] = pNi[niIdx];	
							}
						}
						if (!imap.empty()) //at least one unknown
						{
							matrix_vec.push_back(imap);
							right_vec.push_back(rightVal);
						}
					}
				}
				else  //don't collect matrix_vec to save time
				{
					for (int j = 0; j<Ni.rows; ++j)
					{
						double rightVal = 0.0;
						bool hasUnknown = false;
						for (int k = 0; k<Ni.cols-3; ++k)
						{
							int niIdx = j * Ni.cols + k;

							//sIdx position in original map
							int sy = k/IS_WSIZE - IS_WSIZE_2 + y;
							int sx = k%IS_WSIZE - IS_WSIZE_2 + x;
							//shading value already known
							if (idx_map[sy][sx] == IS_KNOWN)
							{
								rightVal -= pNi[niIdx] * sha_map[sy][sx];								
							}
							else
							{	
								hasUnknown = true;								
							}
						}
						if (hasUnknown) //at least one known
						{							
							right_vec.push_back(rightVal);
						}
					}
				}			
			}
		}
	}		
}

void IntrinsicSolver::GetEquationAtEdge(const Mat &idxMap, const Mat &shadingMap, 
										const Mat &edgeMap, const Mat &orgImage, 
									   vector<map<int, double>> &matrix_vec, 
									   vector<double> &right_vec)
{
	assert(idxMap.depth() == CV_32S);
	assert(shadingMap.depth() == CV_32F);

	int w = orgImage.cols;
	int h = orgImage.rows;	

	Bw8UImage edge_map(&edgeMap);
	Rgb8UImage org_img(&orgImage);
	Bw32FImage sha_map(&shadingMap);
	Bw32SImage idx_map(&idxMap);
	for (int y = IS_WSIZE_2; y<h-IS_WSIZE_2; ++y)
	{
		for (int x = IS_WSIZE_2; x<w-IS_WSIZE_2; ++x)
		{
			//it is an edge
			if (edge_map[y][x] == 255)
			{
				//unknown index numbers
				int idxes[9];
				int colors[9];
				int c_value = 0;
				for (int m=-IS_WSIZE_2; m<=IS_WSIZE_2; m++) 
				{
					int yC = y+m;
					for (int n=-IS_WSIZE_2; n<=IS_WSIZE_2; n++)
					{
						int xC = x+n;
						idxes[(m+1)*3+n+1] = idx_map[yC][xC];
						colors[(m+1)*3+n+1] = 0.3 * org_img[yC][xC].r + 0.59 * org_img[yC][xC].g + 0.11 * org_img[yC][xC].b;
					}
				}

				//3 4 5
				{
					map<int, double> imap;
					double rightVal = 0.0;
					//s3
					if (idxes[3] == -1)
					{
						rightVal -= sha_map[y][x-1] * (colors[4] - colors[5]);
					}
					else
					{
						imap[idxes[3]] = colors[4] - colors[5];
					}

					//s4
					if (idxes[4] == -1)
					{
						rightVal -= sha_map[y][x] * (colors[5] - colors[3]);
					}
					else
					{
						imap[idxes[4]] = colors[5] - colors[3];
					}

					//s5
					if (idxes[5] == -1)
					{
						rightVal -= sha_map[y][x+1] * (colors[3] - colors[4]);
					}
					else
					{
						imap[idxes[5]] = colors[3] - colors[4];
					}
					if (!imap.empty())
					{
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);
					}									
				}

				//1 4 7
				{
					map<int, double> imap;
					double rightVal = 0.0;

					//s1
					if (idxes[1] == -1)
					{
						rightVal -= sha_map[y-1][x] * (colors[4] - colors[7]);
					}
					else
					{
						imap[idxes[1]] = colors[4] - colors[7];
					}

					//s4
					if (idxes[4] == -1)
					{
						rightVal -= sha_map[y][x] * (colors[7] - colors[1]);
					}
					else
					{
						imap[idxes[4]] = colors[7] - colors[1];
					}

					//s7
					if (idxes[7] == -1)
					{
						rightVal -= sha_map[y+1][x] * (colors[1] - colors[4]);
					}
					else
					{
						imap[idxes[7]] = colors[1] - colors[4];
					}
					if (!imap.empty())
					{
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);
					}									
				}
# if 1
				//0 4 8
				{
					map<int, double> imap;
					double rightVal = 0.0;

					//s0
					if (idxes[0] == -1)
					{
						rightVal -= sha_map[y-1][x-1] * (colors[4] - colors[8]);
					}
					else
					{
						imap[idxes[0]] = colors[4] - colors[8];
					}

					//s4
					if (idxes[4] == -1)
					{
						rightVal -= sha_map[y][x] * (colors[8] - colors[0]);
					}
					else
					{
						imap[idxes[4]] = colors[8] - colors[0];
					}

					//s8
					if (idxes[8] == -1)
					{
						rightVal -= sha_map[y+1][x+1] * (colors[0] - colors[4]);
					}
					else
					{
						imap[idxes[8]] = colors[0] - colors[4];
					}
					if (!imap.empty())
					{
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);
					}
				}

				//2 4 6
				{
					map<int, double> imap;
					double rightVal = 0.0;

					//s2
					if (idxes[2] == -1)
					{
						rightVal -= sha_map[y-1][x+1] * (colors[4] - colors[6]);
					}
					else
					{
						imap[idxes[2]] = colors[4] - colors[6];
					}

					//s4
					if (idxes[4] == -1)
					{
						rightVal -= sha_map[y][x] * (colors[6] - colors[2]);
					}
					else
					{
						imap[idxes[4]] = colors[6] - colors[2];
					}

					//s6
					if (idxes[6] == -1)
					{
						rightVal -= sha_map[y+1][x-1] * (colors[2] - colors[4]);
					}
					else
					{
						imap[idxes[6]] = colors[2] - colors[4];
					}
					if (!imap.empty())
					{
						matrix_vec.push_back(imap);
						right_vec.push_back(rightVal);
					}
				}
#endif
			}
		}
	}
}

void IntrinsicSolver::CorrectErrorMap(const Mat &idxMap, const Mat &errorMap, const Mat &rectifyMap, 
						vector<map<int, double>> &matrix_vec, vector<double> &right_vec)
{
	assert(idxMap.depth() == CV_32S);
	assert(rectifyMap.depth() == CV_32F);
	
	int w = idxMap.cols;
	int h = idxMap.rows;

	Bw8UImage e_map(&errorMap);
	Bw32SImage idx_map(&idxMap);
	Bw32FImage rec_map(&rectifyMap);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			if (e_map[y][x] == 255)
			{
				int idx = idx_map[y][x];
				if (idx != -1)
				{
					map<int, double> imap;
					imap[idx] = 1;

					matrix_vec.push_back(imap);
					right_vec.push_back(rec_map[y][x]);
				}
			}
		}
	}

}
